/*
 * @author("Raymond Blum" <raymond@insanegiantrobots.com>)
 * targeted for an ATTiny85 @ 1,8 Mhz but should work on any Arduino compatible controller
 *
 * Copyright (c) 2013 by Raymond Blum
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 **/

#include <SoftwareServo.h> 

SoftwareServo myservo;  // create servo object to control a servo 

#define PIN_LED PB0
#define PIN_SERVO PB1
#define PIN_CDS PB3
#define PIN_PING_TRIG PB2
#define PIN_PING_ECHO PB4

#define ANALOG_READ_DELAY 10
#define POINTED_DELAY 2000

int last_light_level;
#define LIGHT_CHANGE_THRESHOLD 25

#define MAX_PULSE 180
#define MIN_PULSE 0
#define  MOVEMENT_DELAY 3000
#define PULSE_STEP 5

#define DISTANCE_CHANGE_THRESHOLD 10
#define DEFAULT_DISTANCE 100

int hPulse        =   MIN_PULSE;
bool Dir;                         // Servo direction
int max_distance, min_distance, last_distance, prev_distance;  // min and max distance seen during a sweep() and the last reading
int max_location, min_location;  // Servo position at max and min distance readings during a sweep()

int current_sleep_distance;
int previous_sleep_distance;

boolean sweep_complete, pointed;

int sweep_swing_remaining_count;

unsigned long int led_step_at;
unsigned long int shine_end_at;

int led_level;
bool led_dir;
#define LED_MIN 0
#define LED_MAX 12
#define LED_STEP_DURATION 100
#define SHINE_DURATION 3000
#define BLINK_DURATION 200
#define LED_SHINE_BRIGHTNESS 255

void startLedPulsing() {
  led_level = LED_MAX;
  led_dir = false;
  led_step_at = 0;
  analogWrite(PIN_LED, led_level);
}

void expireLed() {
  if (isShining()) {
    return;
  }
  if (shine_end_at != 0) {
    shine_end_at = 0;
    analogWrite(PIN_LED, LED_MIN);
    isLightChanged();     // Throw away a light change now as the LED was shining brightly
    return;
  }
  if (isSweeping()) {
    return;
  }  
  if (millis() <= led_step_at) {
    return;
  }
  led_step_at = millis() + LED_STEP_DURATION;

  if (led_dir) {
    if (led_level < LED_MAX) {
      led_level++;
    } 
    else { 
      led_dir = false;
    }
  } 
  else {
    if (led_level > LED_MIN) {
      led_level--;
    } 
    else { 
      led_dir = true;
    }
  }
  analogWrite(PIN_LED, led_level);
}

boolean isShining() {
  return (shine_end_at != 0) && (shine_end_at > millis());
}

void startShining() {
  if (! isShining()) {
    analogWrite(PIN_LED, LED_SHINE_BRIGHTNESS);
    shine_end_at = millis() + SHINE_DURATION;
  }
}

void blinkLed() {
  if (! isShining()) {
    analogWrite(PIN_LED, LED_SHINE_BRIGHTNESS);
    shine_end_at = millis() + BLINK_DURATION;
  }
}

int getDistance() {
  digitalWrite(PIN_PING_TRIG, LOW); 
  delayMicroseconds(2); 

  digitalWrite(PIN_PING_TRIG, HIGH);
  delayMicroseconds(10); 

  digitalWrite(PIN_PING_TRIG, LOW);

  int duration = pulseIn(PIN_PING_ECHO, HIGH);

  if (duration <= 0) {
    return DEFAULT_DISTANCE;
  }
  //Calculate the distance (in cm) based on the speed of sound.
  float HR_dist = duration/58.2;
  return int(HR_dist);
}

int getBrightness() {
  analogRead(PIN_CDS);
  delay(ANALOG_READ_DELAY);
  return analogRead(PIN_CDS);
}

void pointAt(int loc) {
  for (int pos=0; pos<min_location; pos++) {
    point(pos);
  }
  pointed = true;
}

boolean isDistanceChanged() {
  current_sleep_distance = getDistance();

  boolean movement = abs(current_sleep_distance - previous_sleep_distance) > (DISTANCE_CHANGE_THRESHOLD);
  previous_sleep_distance = current_sleep_distance;
  return movement;
}

void point(int loc) {
  myservo.write(loc);
  delayMicroseconds(MOVEMENT_DELAY);     // waits 15ms for the servo to reach the position 
  SoftwareServo::refresh();
}

void startSweeping() {
  min_distance = DEFAULT_DISTANCE;
  max_distance = DEFAULT_DISTANCE;
  sweep_swing_remaining_count = 2;
  pointed = false;
  stopLedPulsing();
}

void stopLedPulsing() {
  led_level = LED_MIN;
  analogWrite(PIN_LED, led_level);
}

boolean isSweeping() {
  return sweep_swing_remaining_count > 0;
}

void sweep() {
  if (!isSweeping()) {
    return;
  }

  point(hPulse);
  delayMicroseconds(MOVEMENT_DELAY);           // Give servo some time to move before giving it a new position

  if (Dir == 1) {
    if (hPulse < MAX_PULSE) {
      hPulse+=PULSE_STEP;                     // Rotate servo to 180 degrees
    } else {                                  // Servo hit upper limit
      hPulse = MAX_PULSE;                     // Keep servo angle in bounds
      Dir=!Dir;                               // Switch direction
      sweep_swing_remaining_count--;
    }
  } else {
    if (hPulse > MIN_PULSE) {
      hPulse-=PULSE_STEP;                     // Rotate servo to 0 degrees
    } else {                                  // Servo hit lower limit
      hPulse = MIN_PULSE;                     // Keep servo angle in bounds
      Dir=!Dir;                               // switch direction
      sweep_swing_remaining_count--;
    }
  }

  last_distance = getDistance();
  if (last_distance<min_distance) {
    min_distance=last_distance;
    min_location=hPulse;
    blinkLed();
    isLightChanged();      // Throw away a light change as the LED has just started shining brightly
  }
  if (last_distance>max_distance) {
    max_distance=last_distance;
    max_location=hPulse;
  }
  if (!isSweeping()) {
    startLedPulsing();
  }
}

boolean isPointed() {
  return pointed;
}

boolean isLightChanged() {
  int light_level = getBrightness();

  boolean changed = (abs(last_light_level - light_level) > LIGHT_CHANGE_THRESHOLD);
  last_light_level = light_level;
  return changed;
}


void setup() { 
  // Serial.begin(9600);
  pinMode(PIN_PING_TRIG, OUTPUT);
  pinMode(PIN_PING_ECHO, INPUT);
  pinMode(PIN_CDS, INPUT);
  myservo.attach(PIN_SERVO);
  myservo.setMaximumPulse(2200);
  pinMode(PIN_LED, OUTPUT);
  analogWrite(PIN_LED, LED_MIN);
  Dir = 1;
  sweep_swing_remaining_count = 0;
  pointed = true;
  shine_end_at = 0;
  last_light_level = getBrightness();
  startLedPulsing();
  prev_distance = getDistance();
  isDistanceChanged();
}

void loop() {  
  if (!isSweeping()) {
   if (isLightChanged() || isDistanceChanged()) {
     startSweeping();
    }
  }
  
  sweep();
  expireLed();

  if (!isSweeping()) {
    if (!isPointed()) {
      pointAt(min_location);
      startShining();
      isLightChanged();        // Throw away a light change as the LED has just started shining brightly
      delay(POINTED_DELAY);    // Wait for the servo to catch up with the last pulse sent
      isDistanceChanged();     // Throw away a distance change as we've just come to rest
    }
  }
  prev_distance = last_distance;
}
